/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-05-13     Administrator       the first version
 */
#ifndef HARDWARE_BALANCE_H_
#define HARDWARE_BALANCE_H_


#include "system.h"

#define BALANCE_TASK_PRIO       13     //Task priority //任务优先级
#define BALANCE_STK_SIZE        1024   //Task stack size //任务堆栈大小


// 状态机状态定义
enum {
    Find_Situ, //原地寻找，没找到则切换
    Find_circle,//旋转寻找
    Trace,//追玩具
    Judge_one,//判断追踪到了合适的位置点，停下并机械臂切换到抓玩具状态
    Judge_two,//缓慢移动车并判断摄像头中是否出现玩具，出现了则停止车并打开抓的环
    Catch,//抓玩具
    Judge_three,//判断视角内是否还有其它toy
    Put_toy,//放玩具回仓库，判断是否全部抓完了，抓完就找家
    Put_finish,//
    PanDuan,
    Trace_home,//找家
    Judge_one_home,
    Judge_two_home,
    Catch_home,
    Judge_three_home,//收机械臂
    Back_home,//缓慢移动回到家
    Arrive_home,
    Disable//结束
} balance_state;


/*输出四个电机的PWM*/
void Drive_motor(rt_int32_t PWMA,rt_int32_t PWMB,rt_int32_t PWMC,rt_int32_t PWMD);

//保持车身不变，往固定角度行驶
void MOVE(float angle,float speed, float w_z);//speed在这里的单位是圈/秒

/*主控制函数*/
void MAIN_Control(float Vx,float Vy,float Vz);

/*控制任务函数*/
void Balance_task(void *pvParameters);

extern float PWM_A, PWM_B, PWM_C, PWM_D;


#endif /* HARDWARE_BALANCE_H_ */
